/**
 * @file imu_adaption.c
 * @author your name (you@domain.com)
 * @brief 此文件为上层提供统一的API接口，方便模块的更换，如有模块更换，只需要
 *        替换此文件的实现过程，不需要改头文件和上层代码。
 * @version 0.1
 * @date 2022-12-20
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#include "string.h"
#include "adaption_magnetometer.h"
#include "system_support.h"
#include "lsm303d.h"
#include "lsm303d_config.h"
#include "DeviceCharacteristic.h"
#include "math.h"

#define MAG_MODULE_TAG    "magnetom_module"

/* 磁力计灵敏度 */
#define MAG_SENSITIVITY     3.3f
/* uT转高斯的转换系数 */
#define UT2GAUSS			100

#define MAX_MATRIX              4

static void mag_rectify(MagnetometerTypedef *_magnetometer);
static void mag_convert(MagnetometerTypedef *_magnetometer);
static int8_t mag_direction_init(MagnetometerTypedef *_magnetometer);
static void mag_set_offset(MagnetometerTypedef *_magnetometer, float _mag_offset[3]);
static void sphere_fit_add_sample(SphereLeastFitStruct *sphere_fit_struct, struct Point3D *points);
static void sphereLeastFit(SphereLeastFitStruct *sphere_fit_struct);
static void Gauss(double m[][MAX_MATRIX], int n);

int8_t Magnetometer_Init(MagnetometerTypedef *_Magnetometer)
{
    int8_t ret;
    uint8_t who_am_i;

    ret = LSM303D_GetWhoAmI(&who_am_i);
    if(ret < 0){
        elog_error(MAG_MODULE_TAG, "get WAI fail");
        return ret;
    }

    if(who_am_i != LSM303D_WHO_AM_I_ID){
        elog_error(MAG_MODULE_TAG, "WAI wrong:0x%02x", who_am_i);
        return -1;
    }

    ret = LSM303D_SetRegMode();
    if(ret < 0){
        elog_error(MAG_MODULE_TAG, "SetRegMode fail");
        return ret;
    }


    mag_set_offset(_Magnetometer, MagBias);

    ret += mag_direction_init(_Magnetometer);
    if(ret == 0)
        elog_info(MAG_MODULE_TAG, "LSM303D init success");

    return ret;
}

void Magnetometer_GetMagRaw(MagnetometerTypedef *_Magnetometer, int16_t *mag_raw)
{
    for(uint8_t i = 0; i < 3; i ++){
        mag_raw[i] = _Magnetometer->mag_raw[i];
    }
}

void Magnetometer_GetMagRel(MagnetometerTypedef *_magnetometer, float *mag_rel)
{
    for(uint8_t i = 0; i < 3; i ++){
        mag_rel[i] = (float)_magnetometer->mag_rel[i];
    }
}

int8_t Magnetometer_UpdateData(MagnetometerTypedef *_magnetometer)
{
    int8_t ret;
    _3AxisMagDataStruct raw_data;

    /* 记录数据 */
    ret = LSM303D_PollingMagRawData(&raw_data);
    _magnetometer->mag_raw[0] = raw_data.MagData_X;
    _magnetometer->mag_raw[1] = raw_data.MagData_Y;
    _magnetometer->mag_raw[2] = raw_data.MagData_Z;

    if(ret < 0){
        elog_error(MAG_MODULE_TAG, "get data fail");
        return ret;
    }

    /* 记录时间戳 */
    _magnetometer->last_record_time_us = _magnetometer->record_time_us;
    _magnetometer->record_time_us = Get_SystemTimer_us();
    if(_magnetometer->last_record_time_us == 0){
        _magnetometer->derta_record_time_us = 0;
        _magnetometer->update_freq_Hz = 0;
    }
    else{
        _magnetometer->derta_record_time_us = _magnetometer->record_time_us - _magnetometer->last_record_time_us;
        _magnetometer->update_freq_Hz = 1000000 / _magnetometer->derta_record_time_us;
    }

    /* 方向和单位转换 */
    mag_convert(_magnetometer);
    
    // 磁力计校准
    #if CALIBRATION_ENABLE == 1
    struct Point3D sample_point;
    sample_point.x = _magnetometer->mag_convert[0];
    sample_point.y = _magnetometer->mag_convert[1];
    sample_point.z = _magnetometer->mag_convert[2];

    static uint64_t last_sample_time_ms = 0;
    uint64_t sample_time_ms = Get_SystemTimer_ms();

    /* 按照CALIBRATION_SAMPLE_PERIOD_MS所设定的采样周期来采样 */
    if(sample_time_ms - last_sample_time_ms > CALIBRATION_SAMPLE_PERIOD_MS){
        last_sample_time_ms = sample_time_ms;
        sphere_fit_add_sample(&_magnetometer->sphere_fit_handle, &sample_point);
        sphereLeastFit(&_magnetometer->sphere_fit_handle);
    }
    #endif /*CALIBRATION_ENABLE == 1*/

    /* 数据修正 */
    mag_rectify(_magnetometer);
    // _magnetometer->test_yaw = atan2f(_magnetometer->mag_rel[1],_magnetometer->mag_rel[0])*57.3;
    // if(_magnetometer->test_yaw <0.0){
    //     _magnetometer->test_yaw += 360.0f;
    // }
    

	
	_magnetometer->start_record_time_us = Get_SystemTimer_us();

    return ret;
}

 /**
  * @brief 磁力计方向转换
  * 
  * @param _magnetometer 磁力计句柄
  */
static void mag_convert(MagnetometerTypedef *_magnetometer)
{
	/* 方向转换 */
	_magnetometer->mag_convert[0] = _magnetometer->direction[0]*(_magnetometer->mag_raw[_magnetometer->direction[1]]);
	_magnetometer->mag_convert[1] = _magnetometer->direction[2]*(_magnetometer->mag_raw[_magnetometer->direction[3]]);
	_magnetometer->mag_convert[2] = _magnetometer->direction[4]*(_magnetometer->mag_raw[_magnetometer->direction[5]]);

    /* 单位转换 */
    _magnetometer->mag_convert[0] = _magnetometer->mag_convert[0]*LSM_MAG_FCT;
    _magnetometer->mag_convert[1] = _magnetometer->mag_convert[1]*LSM_MAG_FCT;
    _magnetometer->mag_convert[2] = _magnetometer->mag_convert[2]*LSM_MAG_FCT;
}

/**
 * @brief 磁力计数据矫正
 * 
 * @param _magnetometer 磁力计句柄
 */
static void mag_rectify(MagnetometerTypedef *_magnetometer)
{
    /* 磁力计测量值矫正 */
	_magnetometer->mag_rel[0] = _magnetometer->mag_convert[0] - _magnetometer->mag_offset[0];
	_magnetometer->mag_rel[1] = _magnetometer->mag_convert[1] - _magnetometer->mag_offset[1];
	_magnetometer->mag_rel[2] = _magnetometer->mag_convert[2] - _magnetometer->mag_offset[2];
    memcpy(_magnetometer->mag_msg.mag,_magnetometer->mag_rel,sizeof(_magnetometer->mag_rel));
}

/**
 * @brief 字符串转矩阵，将"+X+Y+Z"转成 +1 0 +1 1 +1 2
 * 
 * @param _magnetometer 磁力计句柄
 * @param direction_string 字符串
 * @return int8_t 
 */
static int8_t mag_direction_init(MagnetometerTypedef *_magnetometer)
{
    char *direction_string = MAG_DIRECTION;
    
	for(int i=0;i<6;)
	{
		switch(direction_string[i])
		{
			case '+':_magnetometer->direction[i] = 1;break;
			case '-':_magnetometer->direction[i] =-1;break;
			default:return -1;
		}
		i++;
		switch(direction_string[i])
		{
			case 'X':_magnetometer->direction[i] = 0;break;
			case 'Y':_magnetometer->direction[i] = 1;break;
			case 'Z':_magnetometer->direction[i] = 2;break;
			default:return -1;
		}
		i++;
	}
	return 0;
}

/**
 * @brief 设置磁力计偏置值
 * 
 * @param _magnetometer 
 * @param _mag_offset 
 */
static void mag_set_offset(MagnetometerTypedef *_magnetometer, float _mag_offset[3])
{
    _magnetometer->mag_offset[0] = _mag_offset[0];
    _magnetometer->mag_offset[1] = _mag_offset[1];
    _magnetometer->mag_offset[2] = _mag_offset[2];
}

int8_t Magnetometer_CheckConnect(MagnetometerTypedef *_magnetometer)
{
	uint64_t current_time_us = Get_SystemTimer_us();
	if(_magnetometer->start_record_time_us != 0 && \
		current_time_us - _magnetometer->start_record_time_us > DISCONNECT_TIME_THRESHOLD_MS * 1000)
		return -1;
	else
		return 0;
}



/**
 * @brief 最小二乘法拟合球面，详细推导过程可参照此博客
 * https://blog.csdn.net/sunshine_zoe/article/details/78852978
 * @param points 
 * @param center_x 
 * @param center_y 
 * @param center_z 
 * @param radius 
 * @return true 
 * @return false 
 */
static void sphereLeastFit(SphereLeastFitStruct *sphere_fit_struct)
{
    if(sphere_fit_struct->calibration_status == Completed)
        return;
    if(sphere_fit_struct->point_num >= CALIBRATION_MIN_POINTS)
    {
        double augmented_matrix[3][MAX_MATRIX]; // 由系数矩阵和常向量组成的增广矩阵

        // 求个过程变量的均值
        double avg_x = sphere_fit_struct->process_variables.sum_x / sphere_fit_struct->point_num;
        double avg_y = sphere_fit_struct->process_variables.sum_y / sphere_fit_struct->point_num;
        double avg_z = sphere_fit_struct->process_variables.sum_z / sphere_fit_struct->point_num;

        double avg_xx = sphere_fit_struct->process_variables.sum_xx / sphere_fit_struct->point_num;
        double avg_yy = sphere_fit_struct->process_variables.sum_yy / sphere_fit_struct->point_num;
        double avg_zz = sphere_fit_struct->process_variables.sum_zz / sphere_fit_struct->point_num;
        double avg_xy = sphere_fit_struct->process_variables.sum_xy / sphere_fit_struct->point_num;
        double avg_xz = sphere_fit_struct->process_variables.sum_xz / sphere_fit_struct->point_num;
        double avg_yz = sphere_fit_struct->process_variables.sum_yz / sphere_fit_struct->point_num;

        double avg_xxx = sphere_fit_struct->process_variables.sum_xxx / sphere_fit_struct->point_num;
        double avg_yyy = sphere_fit_struct->process_variables.sum_yyy / sphere_fit_struct->point_num;
        double avg_zzz = sphere_fit_struct->process_variables.sum_zzz / sphere_fit_struct->point_num;
        double avg_xxy = sphere_fit_struct->process_variables.sum_xxy / sphere_fit_struct->point_num;
        double avg_xyy = sphere_fit_struct->process_variables.sum_xyy / sphere_fit_struct->point_num;
        double avg_xxz = sphere_fit_struct->process_variables.sum_xxz / sphere_fit_struct->point_num;
        double avg_xzz = sphere_fit_struct->process_variables.sum_xzz / sphere_fit_struct->point_num;
        double avg_yyz = sphere_fit_struct->process_variables.sum_yyz / sphere_fit_struct->point_num;
        double avg_yzz = sphere_fit_struct->process_variables.sum_yzz / sphere_fit_struct->point_num;

        // 给系数矩阵A赋值
        augmented_matrix[0][0] = avg_xx - avg_x * avg_x;
        augmented_matrix[0][1] = avg_xy - avg_x * avg_y;
        augmented_matrix[0][2] = avg_xz - avg_x * avg_z;
        augmented_matrix[1][0] = avg_xy - avg_x * avg_y;
        augmented_matrix[1][1] = avg_yy - avg_y * avg_y;
        augmented_matrix[1][2] = avg_yz - avg_y * avg_z;
        augmented_matrix[2][0] = avg_xz - avg_x * avg_z;
        augmented_matrix[2][1] = avg_yz - avg_y * avg_z;
        augmented_matrix[2][2] = avg_zz - avg_z * avg_z;

        // 给常向量B赋值
        augmented_matrix[0][3] = (avg_xxx - avg_x * avg_xx \
                                + avg_xyy - avg_x * avg_yy \
                                + avg_xzz - avg_x * avg_zz)/2;
        augmented_matrix[1][3] = (avg_xxy - avg_y * avg_xx \
                                + avg_yyy - avg_y * avg_yy \
                                + avg_yzz - avg_y * avg_zz)/2;
        augmented_matrix[2][3] = (avg_xxz - avg_z * avg_xx \
                                + avg_yyz - avg_z * avg_yy \
                                + avg_zzz - avg_z * avg_zz)/2;

        Gauss(augmented_matrix, 3);

        sphere_fit_struct->fitting_result.center_point.x = augmented_matrix[0][3];
        sphere_fit_struct->fitting_result.center_point.y = augmented_matrix[1][3];
        sphere_fit_struct->fitting_result.center_point.z = augmented_matrix[2][3];

        sphere_fit_struct->fitting_result.radius = \
        sqrt(avg_xx - 2*augmented_matrix[0][3]*avg_x + augmented_matrix[0][3]*augmented_matrix[0][3] \
            +avg_yy - 2*augmented_matrix[1][3]*avg_y + augmented_matrix[1][3]*augmented_matrix[1][3] \
            +avg_zz - 2*augmented_matrix[2][3]*avg_z + augmented_matrix[2][3]*augmented_matrix[2][3]);

        elog_info(MAG_MODULE_TAG, "center point x:%.3f, y:%.3f, z:%.3f, R:%.3f", \
                sphere_fit_struct->fitting_result.center_point.x,  \
                sphere_fit_struct->fitting_result.center_point.y,  \
                sphere_fit_struct->fitting_result.center_point.z,  \
                sphere_fit_struct->fitting_result.radius);

        sphere_fit_struct->calibration_status = Completed;
    }

}

/**
 * @brief 增加一个采样点，利用输入点求各过程变量
 * 
 * @param sphere_fit_struct 
 * @param points 
 */
static void sphere_fit_add_sample(SphereLeastFitStruct *sphere_fit_struct, struct Point3D *points)
{
    if(sphere_fit_struct->calibration_status == Completed)
        return;
    // 初始化变量
    if(sphere_fit_struct->calibration_status == NotInit)
    {
        sphere_fit_struct->calibration_status = NotComplete;
        sphere_fit_struct->point_num = 0;

        // 全部变量初始化为0
        memset(&sphere_fit_struct->fitting_result, 0, sizeof(sphere_fit_struct->fitting_result));
        memset(&sphere_fit_struct->process_variables, 0, sizeof(sphere_fit_struct->process_variables));
    }

    sphere_fit_struct->process_variables.sum_x += points->x;
    sphere_fit_struct->process_variables.sum_y += points->y;
    sphere_fit_struct->process_variables.sum_z += points->z;

    sphere_fit_struct->process_variables.sum_xx += points->x * points->x;
    sphere_fit_struct->process_variables.sum_yy += points->y * points->y;
    sphere_fit_struct->process_variables.sum_zz += points->z * points->z;

    sphere_fit_struct->process_variables.sum_xy += points->x * points->y;
    sphere_fit_struct->process_variables.sum_xz += points->x * points->z;
    sphere_fit_struct->process_variables.sum_yz += points->y * points->z;

    sphere_fit_struct->process_variables.sum_xxx += points->x * points->x * points->x;
    sphere_fit_struct->process_variables.sum_yyy += points->y * points->y * points->y;
    sphere_fit_struct->process_variables.sum_zzz += points->z * points->z * points->z;

    sphere_fit_struct->process_variables.sum_xxy += points->x * points->x * points->y;
    sphere_fit_struct->process_variables.sum_xyy += points->x * points->y * points->y;
    sphere_fit_struct->process_variables.sum_xxz += points->x * points->x * points->z;
    sphere_fit_struct->process_variables.sum_xzz += points->x * points->z * points->z;
    sphere_fit_struct->process_variables.sum_yyz += points->y * points->y * points->z;
    sphere_fit_struct->process_variables.sum_yzz += points->y * points->z * points->z;

    sphere_fit_struct->point_num ++;
}

/**
* @brief SwapRow进行行交换
* @param m 待计算的矩阵
* @param row 待交行的行
* @param max_row 待交换的另一行
* @param n 矩阵行数
*/
static void SwapRow(double m[][MAX_MATRIX], int row, int max_row, int n)
{
    double swap;
    for(int k = row; k <= n; k++){
        swap = m[row][k];
        m[row][k] = m[max_row][k];
        m[max_row][k] = swap;
    }
}

/**
 * @brief 组上三角矩阵
 * @param m 待计算的矩阵
 * @param n 矩阵行数
 */
static void SelectColE(double m[][MAX_MATRIX], int n)
{
    int max_row_e = 0;  //主元所在行
    double ratio = 0;   //消元因数
    for (int j = 0; j < n; j++)
    {
        max_row_e = j;
        for (int i = j; i < n; i++)
        {
            if (fabs(m[i][j]) >fabs(m[max_row_e][j])){
                max_row_e=i;
            }
        }
        if(max_row_e!=j){
            SwapRow(m, j, max_row_e, n);//与最大主元所在行交换
        }
        //消元
        for(int i = j+1; i < n; i++)
        {
            ratio = m[i][j] / m[j][j];
            for (int k = j; k < n + 1; k++)
            {
                m[i][k] -= m[j][k] * ratio;
            }
        }
    }
}

/**
 * @brief: Gauss 高斯列主元消元法求解线性方程(A*X = B)
 * @param: m 由于A|B组成的增广矩阵,X为待求的解
 *         n 求解的元数,n要小于MAX_MATRIX
 * @result:所求结果存放在m[][n]中
 */
static void Gauss(double m[][MAX_MATRIX], int n)
{
    SelectColE(m, n);   // 列选主元并消元成上三角
    // 回代求解,结果存在m[][n]中
    for(int i = n - 1; i >= 0; i--)
    {
        for(int j = i+1; j < n; j++)
        {
            m[i][n] -= m[i][j] * m[j][n];
        }
        m[i][n] /= m[i][i];
    }
}
